#pragma once
#ifndef OBR_MATRIX4X4F_H
#define OBR_MATRIX4X4F_H

#ifndef __CUDA_ARCH__
#include <stdio.h>
#endif
#include "helper_math.h"

namespace OBR
{
/**
 * @brief a simple 4x4 float matrix with basic operators
 *
 */
struct Matrix4x4f
{
    float4 data[4];

    __forceinline__ __device__ __host__ Matrix4x4f(float id = 1.0f)
    {
        data[0] = make_float4(id, 0, 0, 0);
        data[1] = make_float4(0, id, 0, 0);
        data[2] = make_float4(0, 0, id, 0);
        data[3] = make_float4(0, 0, 0, id);
    }

    __forceinline__ __device__ __host__ Matrix4x4f(float *source)
    {
        data[0] = make_float4(source[0], source[1], source[2], source[3]);
        data[1] = make_float4(source[4], source[5], source[6], source[7]);
        data[2] = make_float4(source[8], source[9], source[10], source[11]);
        data[3] = make_float4(source[12], source[13], source[14], source[15]);
    }

    __forceinline__ __device__ __host__ Matrix4x4f(float4 r0, float4 r1, float4 r2, float4 r3)
    {
        data[0] = r0;
        data[1] = r1;
        data[2] = r2;
        data[3] = r3;
    }

    /* ------------------------ elementary arithmetic ----------------------- */

    __forceinline__ __device__ __host__ Matrix4x4f operator+(Matrix4x4f &b)
    {
        return Matrix4x4f(this->data[0] + b.data[0], this->data[1] + b.data[1],
                          this->data[2] + b.data[2], this->data[3] + b.data[3]);
    }

    __forceinline__ __device__ __host__ Matrix4x4f operator-(Matrix4x4f &b)
    {
        return Matrix4x4f(this->data[0] - b.data[0], this->data[1] - b.data[1],
                          this->data[2] - b.data[2], this->data[3] - b.data[3]);
    }

    __forceinline__ __device__ __host__ Matrix4x4f operator*(Matrix4x4f &b)
    {
        return Matrix4x4f(this->data[0] * b.data[0], this->data[1] * b.data[1],
                          this->data[2] * b.data[2], this->data[3] * b.data[3]);
    }

    __forceinline__ __device__ __host__ Matrix4x4f operator/(Matrix4x4f &b)
    {
        return Matrix4x4f(this->data[0] / b.data[0], this->data[1] / b.data[1],
                          this->data[2] / b.data[2], this->data[3] / b.data[3]);
    }

    static __forceinline__ __device__ __host__ Matrix4x4f matmult(Matrix4x4f &l, Matrix4x4f &r)
    {
        Matrix4x4f res;
        Matrix4x4f rt = r.transpose();
        for (size_t r = 0; r < 4; r++)
        {
            for (size_t c = 0; c < 4; c++)
            {
                res(r, c) = dot(l.data[r], rt.data[c]);
            }
        }
        return res;
    }

    /* ---------------- elementary arithmetic with constants ---------------- */

    __forceinline__ __device__ __host__ Matrix4x4f operator+(float v)
    {
        return Matrix4x4f(this->data[0] + v, this->data[1] + v, this->data[2] + v,
                          this->data[3] + v);
    }

    __forceinline__ __device__ __host__ Matrix4x4f operator-(float v)
    {
        return Matrix4x4f(this->data[0] - v, this->data[1] - v, this->data[2] - v,
                          this->data[3] - v);
    }

    __forceinline__ __device__ __host__ Matrix4x4f operator*(float v)
    {
        return Matrix4x4f(this->data[0] * v, this->data[1] * v, this->data[2] * v,
                          this->data[3] * v);
    }

    __forceinline__ __device__ __host__ Matrix4x4f operator/(float v)
    {
        return Matrix4x4f(this->data[0] / v, this->data[1] / v, this->data[2] / v,
                          this->data[3] / v);
    }

    /* ------------------------ get value at position ----------------------- */

    __forceinline__ __device__ __host__ float &operator()(int r, int c)
    {
        float4 *row = this->data + r;
        if (c == 0)
            return row->x;
        else if (c == 1)
            return row->y;
        else if (c == 2)
            return row->z;
        else if (c == 3)
            return row->w;
        return this->data[0].x;
    }

    /* ---------------------- vector / point operation ---------------------- */

    __forceinline__ __device__ __host__ float4 operator*(float4 &v)
    {
        return make_float4(dot(this->data[0], v), dot(this->data[1], v), dot(this->data[2], v),
                           dot(this->data[3], v));
    }

    __forceinline__ __device__ __host__ float3 operator*(float3 &v)
    {
        float4 _v = make_float4(v, 1.0);
        float4 res = this->operator*(_v);
        return make_float3(res / res.w);
    }

    /* --------------------------- linear algebra --------------------------- */

    __forceinline__ __device__ __host__ Matrix4x4f inv()
    {
        float inv[16], det;
        float *m = (float *)this->data;
        int i;

        inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] +
                 m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];

        inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] -
                 m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];

        inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] +
                 m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];

        inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] -
                  m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];

        inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] -
                 m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];

        inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] +
                 m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];

        inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] -
                 m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];

        inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] +
                  m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];

        inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] +
                 m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];

        inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] -
                 m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];

        inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] +
                  m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];

        inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] -
                  m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];

        inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] -
                 m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];

        inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] +
                 m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];

        inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] -
                  m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];

        inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] +
                  m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];

        det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

        if (det == 0)
            return Matrix4x4f();

        float invdet = 1.0 / det;
        for (i = 0; i < 16; i++)
            inv[i] *= invdet;

        return Matrix4x4f(inv);
    }

    __forceinline__ __device__ __host__ Matrix4x4f transpose()
    {
        // float tdata[16] = {0};
        // float *odata = (float *)this->data;
        // for (size_t r = 0; r < 4; r++)
        // {
        //     for (size_t c = 0; c < 4; c++)
        //     {
        //         tdata[r * 4 + c] = odata[c * 4 + r];
        //     }
        // }
        // return Matrix4x4f(tdata);
        return Matrix4x4f(make_float4((*this)(0, 0), (*this)(1, 0), (*this)(2, 0), (*this)(3, 0)),
                          make_float4((*this)(0, 1), (*this)(1, 1), (*this)(2, 1), (*this)(3, 1)),
                          make_float4((*this)(0, 2), (*this)(1, 2), (*this)(2, 2), (*this)(3, 2)),
                          make_float4((*this)(0, 3), (*this)(1, 3), (*this)(2, 3), (*this)(3, 3)));
    }

    /* ------------------------------ printing ------------------------------ */

    __forceinline__ __device__ __host__ void print()
    {
        printf("[[%8.4f, %8.4f, %8.4f, %8.4f],\n", this->data[0].x, this->data[0].y,
               this->data[0].z, this->data[0].w);
        printf(" [%8.4f, %8.4f, %8.4f, %8.4f],\n", this->data[1].x, this->data[1].y,
               this->data[1].z, this->data[1].w);
        printf(" [%8.4f, %8.4f, %8.4f, %8.4f],\n", this->data[2].x, this->data[2].y,
               this->data[2].z, this->data[2].w);
        printf(" [%8.4f, %8.4f, %8.4f, %8.4f]]\n", this->data[3].x, this->data[3].y,
               this->data[3].z, this->data[3].w);
    }
};

} // namespace OBR

#endif